package com.robotic.udp;

import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.beans.factory.annotation.Value;
import org.springframework.scheduling.annotation.EnableScheduling;
import org.springframework.scheduling.annotation.Scheduled;
import org.springframework.stereotype.Service;

import java.util.Collections;
import java.util.HashMap;
import java.util.Map;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.atomic.AtomicLong;

@Service
@EnableScheduling
public class RobotService {

    @Autowired
    private UdpCommandSender udpSender;

    @Value("${robot.motion.host}")
    private String motionHost;
    @Value("${robot.motion.port}")
    private int motionPort;
    @Value("${robot.perception.host}")
    private String perceptionHost;
    @Value("${robot.perception.port}")
    private int perceptionPort;

    // 使用 Java 8 兼容的方式初始化 Map
    private static final Map<String, Integer> COMMAND_CODES;
    static {
        Map<String, Integer> map = new HashMap<>();
        map.put("heartbeat", 0x21040001);
        map.put("connect", 0x21020001);
        map.put("stand", 0x21010202);
        map.put("force", 0x2101020A);
        map.put("step", 0x21010201);
        map.put("assist", 0x21010C03);
        COMMAND_CODES = Collections.unmodifiableMap(map);
    }

    private static final int CMD_SET_SPEED_SOURCE = 0x3101EE03;
    private static final int CMD_AXIS_LY = 0x21010130;
    private static final int CMD_AXIS_LX = 0x21010131;
    private static final int CMD_AXIS_RX = 0x21010135;

    private static final int SLOW_JOYSTICK_VALUE = 14746;
    private static final long MOVEMENT_TIMEOUT_MS = 500;

    private final AtomicLong lastMovementCommandTimestamp = new AtomicLong(0);
    private final AtomicInteger forwardSpeed = new AtomicInteger(0);
    private final AtomicInteger strafeSpeed = new AtomicInteger(0);
    private final AtomicInteger turnSpeed = new AtomicInteger(0);

    // volatile 保证可见性，但同步仍需 synchronized
    private volatile boolean isConnected = false;

    // 定时任务1: 维持心跳 (每400ms)
    @Scheduled(fixedRate = 400)
    public synchronized void sendHeartbeat() {
        if (!isConnected) {
            return;
        }
        udpSender.sendCommand(motionHost, motionPort, COMMAND_CODES.get("heartbeat"), 0);
    }

    // 定时任务2: 持续发送移动指令 (每20ms, 50Hz)
    @Scheduled(fixedRate = 20)
    public synchronized void sendMovementCommands() {
        if (!isConnected) {
            return;
        }
        if (System.currentTimeMillis() - lastMovementCommandTimestamp.get() > MOVEMENT_TIMEOUT_MS) {
            forwardSpeed.set(0);
            strafeSpeed.set(0);
            turnSpeed.set(0);
        }
        udpSender.sendCommand(motionHost, motionPort, CMD_AXIS_LY, forwardSpeed.get());
        udpSender.sendCommand(motionHost, motionPort, CMD_AXIS_LX, strafeSpeed.get());
        udpSender.sendCommand(motionHost, motionPort, CMD_AXIS_RX, turnSpeed.get());
    }

    // 连接方法
    public synchronized void connect() {
        if (!isConnected) {
            System.out.println("正在连接到机器人...");
            this.isConnected = true;
            udpSender.sendCommand(motionHost, motionPort, COMMAND_CODES.get("connect"), 0);
            System.out.println("连接确认指令已发送。");
        }
    }

    // 断开连接方法
    public synchronized void disconnect() {
        if (isConnected) {
            System.out.println("正在从机器人断开连接...");
            this.isConnected = false;
            stopMovement(); // 立即停止所有移动
        }
    }

    // 执行状态指令
    public synchronized void executeStateCommand(String command) {
        if (!isConnected) {
            System.err.println("错误：未连接到机器人，无法发送指令: " + command);
            return;
        }
        if ("assist".equalsIgnoreCase(command)) {
            System.out.println("正在激活辅助模式...");
            udpSender.sendCommand(motionHost, motionPort, COMMAND_CODES.get("assist"), 0);
            try { Thread.sleep(100); } catch (InterruptedException e) { Thread.currentThread().interrupt(); }
            udpSender.sendCommand(perceptionHost, perceptionPort, CMD_SET_SPEED_SOURCE, 1);
        } else if (COMMAND_CODES.containsKey(command.toLowerCase())) {
            System.out.println("执行指令: " + command);
            udpSender.sendCommand(motionHost, motionPort, COMMAND_CODES.get(command.toLowerCase()), 0);
        }
    }

    // 更新移动意图
    public synchronized void updateMovement(String direction) {
        if (!isConnected) return;
        lastMovementCommandTimestamp.set(System.currentTimeMillis());
        // 清零其他方向的速度，确保每次只朝一个方向移动
        forwardSpeed.set(0);
        strafeSpeed.set(0);
        turnSpeed.set(0);

        switch (direction.toLowerCase()) {
            case "w": forwardSpeed.set(SLOW_JOYSTICK_VALUE); break;
            case "s": forwardSpeed.set(-SLOW_JOYSTICK_VALUE); break;
            case "a": strafeSpeed.set(-SLOW_JOYSTICK_VALUE); break;
            case "d": strafeSpeed.set(SLOW_JOYSTICK_VALUE); break;
            case "q": turnSpeed.set(-SLOW_JOYSTICK_VALUE); break;
            case "e": turnSpeed.set(SLOW_JOYSTICK_VALUE); break;
        }
    }

    // 停止移动
    public synchronized void stopMovement() {
        System.out.println("执行指令: stop");
        lastMovementCommandTimestamp.set(0); // 立即超时
        forwardSpeed.set(0);
        strafeSpeed.set(0);
        turnSpeed.set(0);
    }

    /**
     * 【新增】 获取当前连接状态的方法
     * @return 如果已连接则返回 true，否则返回 false
     */
    public synchronized boolean isConnected() {
        return this.isConnected;
    }
}
